/*
 * Motor_Control_Init.h
 *
 *  Created on: Mar 25, 2024
 *      Author: Yiwen Zhu
 */

#ifndef SELF_DEFINE_MOTOR_CONTROL_INIT_H_
#define SELF_DEFINE_MOTOR_CONTROL_INIT_H_

#include "clarke.h"
#include "Motor_Control_Fcns.h"
#include "dac.h"
#include "main.h"
#include "svgen.h"
#include "park.h"
#include "pid_reg3.h"
#include "ipark.h"
#include "kalman.h"
#include "simplified_svm.h"
#include "mpcc.h"
#include "math.h"
#include "arm_math.h"
#include "i2c_as5600_dma.h"
#include "cordic_calc.h"
#include "mpdtc.h"
#include "hf_inj.h"

// ########################################################
// ###################### USER SETTINGs ###################
// CURRENT_LOOP || OPEN_LOOP || INIT_ANGLE || MPCC_LOOP || MPDTC_LOOP
// Choose a loop level
#define DEF_CONTROL_LEVEL  	( CURRENT_LOOP )
// ARM_MATH_H || CORDIC_ZO || MATH_H
#define MATH_LIB_TYPE		( ARM_MATH_H )
// DA output
#define DEF_DA_DATA		( (G_Theta)*500 ) 	//AD_BUF[0] (est1.Ld*10000)*100
#define DEF_DA_DATA2	( (hfi1.Theta)*500 ) 	//AD_BUF[0] (est1.Lq*10000)*100 (SAMPLE_D_I/2)+2048
// Usart output
#define SEND_DATA_1		((six_puls1.Step *1000)) // ( (mpdtc1.Ref_Flux *1000)*1000 +1000 )	//
#define SEND_DATA_2		((clarke1.Bs*1000)+1000) // ( (mpdtc1.Fdb_Flux *1000)*1000 +1000 )	//
#define SEND_DATA_3		( G_Theta*1000  )	//(park1.Ds*1000)+1000
#define SEND_DATA_4		( (hfi1.Theta)*1000  )	//( six_puls1.Theta*1000 ); //

// ########################################################
// ########################################################

// #############################################################
// ##############  Controller parameters setting  ##############
#define	IMAX	(0.6f)
#define SPD_KP  (1.1f)//
#define SPD_KI  (0.005f)//
#define	SPD_PI_MAX 	(IMAX)
#define	SPD_PI_MIN 	(-IMAX)

#define	 UMAX_PU	(0.66f)

#define  ID_KP		(0.2f)//
#define  ID_KI		(0.44f)//
#define	 ID_PI_MAX 	(UMAX_PU*DRV_UDC)
#define	 ID_PI_MIN 	(-UMAX_PU*DRV_UDC)

#define  IQ_KP 		(ID_KP)//
#define  IQ_KI  	(ID_KI)//
#define	 IQ_PI_MAX 	(UMAX_PU*DRV_UDC)
#define	 IQ_PI_MIN 	(-UMAX_PU*DRV_UDC)

#define  WEIGHT_FACTOR_TRQUE	(0.03f)

#define HFI_PLL_KP	(0.4)	//	0.6
#define HFI_PLL_KI	(0.005)	//0.006

// ########################################################
// #################### Motor parameters ##################
// Small motor_2804 7NP
#define M_ANG_OFFSET	(547)	// Offset of the sensor  (547)
#define M_NP			(7.0f)		// Pole pairs
#define M_Ld			(0.001455f)	// q-axis inductance (0.004707f)	(0.00097f)
#define M_Lq			(0.001695f)	// d-axis inductance (0.005451f)	(0.00125f)
#define M_R				(5.75f)		// Phase resistance	 (5.75f)		(3.2f)
#define M_PSIr			(0.0045f)	// PM flux	(0.0128f New) (0.0054f old 2804)
#define M_TRQ_RATED		(0.02f)		// Rated torque
// ########################################################
// ########################################################


// #########################################################################
// ################## Constants relate to the hardware #####################
// DC Bus and control frequency
#define DRV_UDC	(7.0f)	// DC bus voltage
#define DRV_TS	(1.0f/CONTROL_FRQ) // Control frequency
// Currtne Sample
#define DEF_RIS_SAMPLE 	(0.05f) // Current sample resistance value
#define DEF_INA240A2_MUL	(50.0f)	// INA240_A2: Vo = Vin*50
// Convert ADC sample value to real current value (0.00107448)
#define DEF_AD_TO_REAL_VDC	(0.00324149f) // (3702--12V; 2776--9V)
#define DEF_AD_TO_REAL_CURRENT_A 	( 3.3f /(DEF_INA240A2_MUL *4095.0f *DEF_RIS_SAMPLE) )
#define DEF_AD_TO_REAL_CURRENT_B 	( 3.3f /(DEF_INA240A2_MUL *4095.0f *DEF_RIS_SAMPLE) )
// AD offset
#define DEF_OFFSET_AD_1  (2048) // Current sample offset of Phase A (AD_BUF[0])
#define DEF_OFFSET_AD_2  (2050) // Current sample offset of Phase B (AD_BUF[1])
// #########################################################################
// #########################################################################

// ########################################################
// #################### Global varibles ###################
// Global structures
extern CLARKE clarke1;
extern IPARK ipark1;
extern SVGEN svm1;
extern PIDREG3 pi_spd1;
extern PIDREG3 pi_id1;
extern PIDREG3 pi_iq1;
extern PARK	park1;
extern SIMP_SVM simp_svm1;
extern MPCC	mpcc1;
extern MPDTC mpdtc1;
extern HFI	hfi1;
extern SIX_PULES six_puls1;

// Global variables
extern int16_t AD_BUF[4];
extern volatile float G_Theta;
extern volatile float G_We;
extern volatile uint16_t G_Theta_Mech;
extern char DEF_CTL_STATE;
// ########################################################
// ########################################################


// #############################################################################
// ########################  User do not change below	########################
// #############################################################################
// Define the constant value do not change
//#define PI	 (3.1415926535f)	// defined in arm_math.h
#define DUAL_PI  (2.0f*PI)
#define DUAL_PI_ELEC	(DUAL_PI*M_NP)
#define DEF_ANG_TO_DUAL_PI	(M_NP*DUAL_PI /4029.0f ) // Product this, for converter the 12bit angle sensor to 0-2pi electrical angle
#define DEF_ANG_OFFEST_DUAL_PI	(M_ANG_OFFSET*DEF_ANG_TO_DUAL_PI)
// Timer ARR value
#define DEF_SVM_TO_CMP (TIM_ARR)

// Calculate sin/cos
#define CORDIC_ZO				2
#define ARM_MATH_H              1
#define MATH_H                	0
#if MATH_LIB_TYPE == ARM_MATH_H
	#define SIN_MACRO(v)	arm_sin_f32((float)v)
	#define COS_MACRO(v)	arm_cos_f32((float)v)
	#define SQRT_MACRO(v)	sqrtf((float)(v))
#elif MATH_LIB_TYPE == MATH_H
	#define SIN_MACRO(v)	sinf((float)v)
	#define COS_MACRO(v)	cosf((float)v)
	#define SQRT_MACRO(v)	sqrtf((float)(v))
#elif MATH_LIB_TYPE == CORDIC_ZO
	#define SIN_MACRO(v)	cordic_q31_sinf((float)v)	// cordic_q15_sinf((float)v)
	#define COS_MACRO(v)	cordic_q31_cosf((float)v)	// cordic_q15_cosf((float)v)
	#define SQRT_MACRO(v)	sqrtf((float)(v))
#endif


// Enum the states of state-machine
enum STATE {
	IDLE = 0, 	// 持续空闲
	RUN ,		// 持续运行
	STOP,		// 暂态，在这个周期禁止输出清空变量
	START,		// 暂态，在这个周期启动输出更新控制参数
};

// Enum the state machine states
#define INIT_ANGLE		0	// Test the angle offset of hall sensor
#define	OPEN_LOOP 		1	// Open loop voltage output
#define	CURRENT_LOOP	2	// Current loop FOC
#define	SPEED_LOOP		3	// Speed-current loop FOC
#define MPCC_LOOP		4	// Model predictive current control
#define MPDTC_LOOP		5	// Model predictive direct torque and flux control

#if(DEF_CONTROL_LEVEL == CURRENT_LOOP)
	#define DEF_MAIN_CONTROL_FUNCTION 	CTL_current_loop_foc_task();
#elif(DEF_CONTROL_LEVEL == MPCC_LOOP)
	#define DEF_MAIN_CONTROL_FUNCTION 	CTL_current_loop_MPCC_task();
	#define DEF_MPC_PARAM_UPDATE_FUNCTION	mpcc_param_update_online();
#elif(DEF_CONTROL_LEVEL == OPEN_LOOP)
	#define DEF_MAIN_CONTROL_FUNCTION 	CTL_openloop_foc_task();
#elif(DEF_CONTROL_LEVEL == INIT_ANGLE)
	#define DEF_MAIN_CONTROL_FUNCTION 	CTL_init_ang();
#elif(DEF_CONTROL_LEVEL ==  MPDTC_LOOP)
	#define DEF_MAIN_CONTROL_FUNCTION 	CTL_current_loop_MPDTC_task();
	#define DEF_MPC_PARAM_UPDATE_FUNCTION	mpdtc_param_update_online();
#else
	#define DEF_MAIN_CONTROL_FUNCTION 	CTL_current_loop_foc_task();
#endif

// #############################################################################
// #############################################################################



// ###############  Functions  ##################
void STATE_clear_ctl_vrbs(void);
void STATE_ctl_struc_init(void);
void STATE_ctl_commond(u8 state);
void STATE_output_state(u8 state);
void STATE_motor_control_state_machine(u8 state);

#endif /* SELF_DEFINE_MOTOR_CONTROL_INIT_H_ */
